#include "CommandThread.h"


xQueueHandle CommandThreadQueue;
extern 	xQueueHandle UARTQueue;



void CommandThreadTaskCode( void * pvParameters ){


	//infinite loop
	//unsigned char buff[2];
	// check if something is received from the queue
		static Comms_Msg_Struct *Msg;
		static Comms_Msg_Struct MotorControllMsg;
		MotorControllMsg.msgType = 0x04;
       	MotorControllMsg.counter = 0;
       	MotorControllMsg.size = 2;
       	MotorControllMsg.data[0] = 0;
       	MotorControllMsg.data[1] = 0;
       	MotorControllMsg.data[2] = 0;
       	MotorControllMsg.data[3] = 0;
       	MotorControllMsg.data[4]  = 0;
       	MotorControllMsg.data[5]  = 0;
	unsigned short distance;
	for(;;){

		
	 	if( CommandThreadQueue != 0 )
    	{
        // Receive a message on the created queue.  Block for 0 ticks if a
        // message is not immediately available.
        	if( xQueueReceive( CommandThreadQueue,(void*) &( Msg ), ( portTickType ) 10 ) )
        		{
					if(Msg->msgType	== ULTRASONIC){
				   // check what type of message is it, and react

				   
				   distance = (unsigned short) Msg->data[1];
				   //distance = distance<<8 + Msg->data[0];
				   if(distance <4){
					MotorControllMsg.data[0] =	0x40;
					MotorControllMsg.data[1] = 0xF0;
				   }else if(distance >7){
				   	MotorControllMsg.data[0] =	0x70;
					MotorControllMsg.data[1] = 0xC0;

				   }else{
				   	MotorControllMsg.data[0] =	0x60;
					MotorControllMsg.data[1] = 0xE0;

				   }
					Msg = &MotorControllMsg;
				   if( UARTQueue != 0 )
	  					{ 				 
      						if( xQueueSend( UARTQueue, ( void * ) &Msg, ( portTickType ) 0 ) != pdPASS )
       								{
           						 // Failed to post the message
	    							}
						}



				   
				   /*
				   switch(Msg->msgType){

						case ULTRASONIC:
							
							break;
						case COLOR:

							break;
						case ENCODER:

							break;
						case MOTORCONTROLLER:

							break;
						default:
            				
					}
					*/
					}



				  // }
				   /*
				   
				   */
				}

		}


	}



}


void Init_CommandThread(){
xTaskHandle CommandThreadHandle;
//static unsigned char ucParameterToPass;	
  /* Create the task, storing the handle.  Note that the passed parameter 
  ucParameterToPass must exist for the lifetime of the task, so in this 
  case is declared static.  If it was just an an automatic stack variable 
  it might no longer exist, or at least have been corrupted, by the time
  the new task attempts to access it. */
  xTaskCreate( CommandThreadTaskCode, ( signed char * ) "Command Thread Task", 2*configMINIMAL_STACK_SIZE, NULL, tskIDLE_PRIORITY
  , &CommandThreadHandle );
  CommandThreadQueue = xQueueCreate( 10, sizeof(Comms_Msg_Struct)  );

}